#pragma once
#include <Plane.h>
#include <constants.h>

class PlaneParas
{
    using Vector3d = Eigen::Vector3d;
    using Matrix = Eigen::Matrix4d;

public:
    PlaneParas(){}
    PlaneParas(const Plane& p) : 
        A(p.getA()), B(p.getB()), C(p.getC()), D(-p.getD()),
        center(p.getMeanPoint()), normal(p.getPlaneNormalVec()){}

    double getDistanceFromPoint(const Vector3d& point) const {
        return std::abs(A * point(0) + B * point(1) + C * point(2) + D)
            / sqrt(pow(A, 2.0) + pow(B, 2.0) + pow(C, 2.0));
    }

    void operator=(const PlaneParas& p)
    {
        A = p.A;
        B = p.B;
        C = p.C;
        D = p.D;
        normal = p.normal;
        center = p.center;
    }

    void transformCenter(const Matrix &t)
    {
        Eigen::Vector4d meanPoint4;
        meanPoint4.topRightCorner(3, 1) = center;
        meanPoint4(3) = 1;
        center = (t*meanPoint4).topRightCorner(3,1);
    }

    void transformParas(const Matrix &t)
    {
        // 要先转换center, 因为平面参数D的计算要依赖于center
        Eigen::Vector4d meanPoint4;
        meanPoint4.topRightCorner(3, 1) = center;
        meanPoint4(3) = 1;
        center = (t*meanPoint4).topRightCorner(3,1);

        // 转换normal
        auto rotMatrix = t.topLeftCorner(3, 3);
        normal = rotMatrix * normal;
        A = normal(0);
        B = normal(1);
        C = normal(2);
        D = -(A*center(0) + B*center(1) + C*center(2));
    }

    // 就是同一个平面
    bool isEqualTo(const PlaneParas &plane) const
    {
        return get_angle(normal, plane.normal) < MAX_ANGLE_BETWEEN_EQUAL_PlANES
            && abs(D - plane.D) < D_THRESHOLD_BETWEEN_EQUAL_PLANES;
    }

    // 可能在同一个平面
    bool isCloseTo(const PlaneParas &plane) const
    {
        return get_distance(center, plane.center) < THRESHOLD_BETWEEN_PLANE_CENTER;
    }

    float A;
    float B;
    float C;
    float D;
    Vector3d normal;
    Vector3d center;

private:
    // 计算两个点的距离
    double get_distance (const Eigen::Vector3d& p1, const Eigen::Vector3d& p2) const
    {
        return sqrt( pow(p1.x()-p2.x(), 2.0) 
                + pow(p1.y()-p2.y(), 2.0)
                + pow(p1.z()-p2.z(), 2.0));
    }

    // 计算两个向量的夹角
    double get_angle (const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) const
    {
        return r2d(atan2(v1.cross(v2).norm(), v1.transpose() * v2));
    }

    double get_angle2(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
    {
        double x = (v1.transpose()*v2);
        return  r2d(acos( x / ( v1.norm()*v2.norm() ) ));
    }
};
